//
// Created by engineai on 2024/07/03.
//

#ifndef ZQ_HUMANOID_ACTUATORMODEL_H
#define ZQ_HUMANOID_ACTUATORMODEL_H

#include "../Utilities/utilities.h"

/*!
 * A model of an actuator containing friction and electrical effects
 */
template <typename T>
class ActuatorModel
{
public:
    /*!
     * Construct a new actuator model with the given parameters
     * @param gearRatio : Gear reduction
     * @param motorKT : Value of KT (torque constant) for the motor
     * @param motorR : Motor resistance
     * @param batteryV : Battery voltage
     * @param damping : Actuator damping (at the joint, Nm/(rad/sec))
     * @param dryFriction : Actuator dry friction (at the joint, Nm)
     * @param tauMax : Maximum torque output of the actuator
     */
    ActuatorModel(T gearRatio, T motorKT, T motorR, T batteryV, T damping,
                  T dryFriction, T tauMax)
        : _gr(gearRatio),
          _kt(motorKT),
          _R(motorR),
          _V(batteryV),
          _damping(damping),
          _dryFriction(dryFriction),
          _tauMax(tauMax) {}

    ActuatorModel() {}

    // compute

    /*!
     * Compute actual actuator torque, given desired torque and speed.
     * takes into account friction (dry and damping), voltage limits, and torque
     * limits
     * @param tauDes : desired torque
     * @param qd : current actuator velocity (at the joint)
     * @return actual produced torque
     */
    T getTorque(T tauDes, T qd)
    {
        // compute motor torque
        T tauDesMotor = tauDes / _gr;       // motor torque
        T iDes = tauDesMotor / (_kt * 1.5); // i = tau / KT
        // T bemf =  qd * _gr * _kt * 1.732;     // back emf
        T bemf = qd * _gr * _kt * 2.;      // back emf
        T vDes = iDes * _R + bemf;         // v = I*R + emf
        T vActual = coerce(vDes, -_V, _V); // limit to battery voltage
        T tauActMotor =
            1.5 * _kt * (vActual - bemf) / _R; // tau = Kt * I = Kt * V / R
        T tauAct = _gr * coerce(tauActMotor, -_tauMax, _tauMax);

        // add damping and dry friction
        if (_frictionEnabled)
            tauAct = tauAct - _damping * qd - _dryFriction * sgn(qd);

        return tauAct;
    }

    /*!
     * Control friction effects
     * @param enabled : enable/disable both dry and damping friction terms
     */
    void setFriction(bool enabled) { _frictionEnabled = enabled; }

private:
    T _gr, _kt, _R, _V, _damping, _dryFriction, _tauMax;
    bool _frictionEnabled = true;
};
#endif // ZQ_HUMANOID_ACTUATORMODEL_H
